import telnetlib
import time
import socket


class RobotController:
    def __init__(self, host="192.168.0.2", port="23") -> None:
        super().__init__()
        self.host = host
        self.port = port
        self.default_timeout = 20
        self.tn = telnetlib.Telnet(host, port)

    def cmd(self, cmd, time_out=2000):
        self.tn.write(cmd.encode('ascii'))
        if time_out is None:
            time_out = self.default_timeout
        response = self.tn.read_until('END'.encode('ascii'), timeout=time_out).decode('ascii')
        if 'END' in response:
            print('cmd complete!')
        elif 'OK' in response:
            print('OK')
        elif 'NG' in response:
            print('NG!')

    def power_on(self, time_out):
        command = '@MOTOR ON\r\n'
        self.cmd(command, time_out=time_out)

    def power_off(self, time_out):
        command = '@MOTOR OFF\r\n'
        self.cmd(command, time_out=time_out)

    def move(self,
             move_type,
             P_list,
             hand=[1, 0, 0],
             S=-1,
             DS=-1,
             VEL=-1,
             time_out=2000):

        # P_list
        # P = [p1, p2, p3, p4, p5, p6]
        n = 96
        move_P_list = []
        for P in P_list:
            P_name = 'P' + str(n)
            command = '@' + P_name
            for p in P:
                command = command + ' ' + str(p)
            self.cmd(command)
            move_P_list.append(P_name)
            n = n + 1

        command = '@MOVE '
        # set move_type, move_type in P(点到点), L(直线插补), C(圆弧插补)
        command = command + move_type + ','
        for P_name in move_P_list:
            command = command + ' ' + P_name
        # todo
        # hand
        hand = hand
        for f in hand:
            command = command + ' ' + str(f)
        if VEL != -1:
            command = command + ', VEL=' + str(VEL)
        elif S != -1:
            command = command + ', S=' + str(S)
        elif DS != -1:
            command = command + ', DS=' + str(DS)
        self.cmd(command, time_out)


def dianrong_move(RC, y, start_time):
    # P4 起始位
    # RC.cmd('@P4 = -400.0 00.0 0.0 0 0 0\r\n', time_out=2)

    command = '@P5 = 100.0 {} 0.0 0 0 0\r\n'.format(str(round(y, 3)))
    RC.cmd(command, time_out=2)
    command = '@P6 = 200.0 {} 87.0 0 0 0\r\n'.format(str(round(y, 3)))
    RC.cmd(command, time_out=2)
    command = '@P7 = 300.0 {} 0.0 0 0 0\r\n'.format(str(round(y, 3)))
    RC.cmd(command, time_out=2)
    while time.time() < start_time:
        continue
    RC.cmd('@MOVE PTP, P5, S=50\r\n', time_out=20)
    RC.cmd('@MOVE L, P6, P7, VEL=50\r\n', time_out=20)
    RC.cmd('@MOVE PTP, P4, S=50\r\n', time_out=20)

if __name__ == '__main__':
    host = "192.168.0.2"
    port = "23"



    #
    # # 创建 Telnet 连接
    # tn = telnetlib.Telnet(host, port)
    # # tn.interact()
    #
    # # 发送指令
    # command = "@MOVE L, P4, P5, S=100\r\n"
    # print(command.encode('ascii'))
    # tn.write(command.encode('ascii'))
    # response = tn.read_until('END'.encode('ascii'), timeout=20)
    # print(response)
    # command = "@MOVE L, P4, P5, S=100\r\n"
    # print(command.encode('ascii'))
    # tn.write(command.encode('ascii'))
    # response = tn.read_until('END'.encode('ascii'), timeout=2000)
    # print(response)
    RC = RobotController(host, port)
    # RC.cmd('@MOTOR ON\r\n', time_out=20)
    #
    # # RC.cmd('@P4 = -500.0 350.0 0 0 0 0\r\n', time_out=2)
    #
    # RC.cmd('@MOVE L, P5, VEL=1000\r\n', time_out=20)
    # t0 = time.time()
    # RC.cmd('@P6 = -400.0 350.0 85.0 0 0 0\r\n', time_out=2)
    # t1 = time.time()
    # RC.cmd('@P7 = -300  350.0 0 0 0 0\r\n', time_out=2)
    # t2 = time.time()
    # print(t2 - t1, t1 - t0)
    # RC.cmd('@MOVE L, P4, VEL=1000\r\n', time_out=20)
    # # RC.cmd('@MOVE L, P6, VEL=50\r\n', time_out=20)
    # RC.cmd('@MOVE L, P6, P7, VEL=50\r\n', time_out=20)
    #
    # RC.cmd('@MOVE L, P5, VEL=50\r\n', time_out=20)
    # RC.cmd('@MOTOR OFF\r\n', time_out=20)

    # dianrong_move(RC)
    RC.cmd('@P4 = -400.0 00.0 0.0 0 0 0\r\n', time_out=2)
    RC.cmd('@MOVE PTP, P4, S=50\r\n', time_out=20)